#include "PmdFile.h"

PmdBone::PmdBone()
{
	for (int n=0;n<20;n++) szName[n] = ' ';
	parentNo = -1;
	childNo = -1;
	kind = -1;
	ikTarget = -1;
	pos = Vector3f::Zero();
	parent = 0;
	child = 0;
	localPos = Vector3f::Zero();
	posOffset = Vector3f::Zero();
	rotOffset = Vector4f::Zero();
	rotOffset << 0,0,0,1;
}

PmdBone::~PmdBone()
{
	parent = 0;
	child = 0;
}

Vector3f PmdBone::calcGlobalPos()
{
	if (parent) return parent->calcGlobalPos(localPos + posOffset);
	else return (localPos + posOffset);
}

Vector3f PmdBone::calcGlobalPos(Vector3f lPos)
{
	Matrix3f rotMat = quat2matrix(rotOffset(0),rotOffset(1),rotOffset(2),rotOffset(3));
	Vector3f newPos = (localPos + posOffset) + rotMat*lPos;
	if (parent) return parent->calcGlobalPos(newPos);
	else return newPos;
}

Matrix3f PmdBone::quat2matrix(double e1, double e2, double e3, double e4)
//Matrix3f VMDdev::quat2matrix(Vector4f ee);
{
	Matrix3f mat;
	//double e1 = ee(0); double e2 = ee(1); double e3 = ee(2); double e4 = ee(3);
	mat << e1*e1 - e2*e2 - e3*e3 + e4*e4, 2.0*( e1*e2 - e3*e4),            2.0*( e1*e3 + e2*e4),
		   2.0*(e1*e2 + e3*e4),           - e1*e1 + e2*e2 - e3*e3 + e4*e4, 2.0*(e2*e3 - e1*e4),
		   2.0*(e1*e3 - e2*e4),           2.0*(e2*e3 + e1*e4),             - e1*e1 - e2*e2 + e3*e3 + e4*e4;
	return mat;
}

PmdFile::PmdFile()
	: QObject()
{
	m_data = 0;
}

PmdFile::~PmdFile()
{
	if (m_data) free(m_data);
}

void PmdFile::readFile(QString filePath)
{
	QFile file(filePath);
	if (file.open(QIODevice::ReadOnly)) {
		QDir dir(filePath);
		filePath.chop(dir.dirName().size());
		m_path = filePath;
		m_fileSize = file.size();
		char* dataPtr = (char*)malloc(m_fileSize);
		QDataStream buffer(&file);
		buffer.setByteOrder(QDataStream::LittleEndian);
		int bytesRead = buffer.readRawData(dataPtr,m_fileSize);
		file.close();

		if (bytesRead < m_fileSize) qDebug() << "Didn't read everything for some reason";

		m_data = (unsigned char*)dataPtr;

		
		PMD_Header* headerPtr = (PMD_Header*)m_data;

		dataPtr += sizeof(PMD_Header);

		// Load Vertices

		int vertexCount = *((unsigned long*)dataPtr);
		dataPtr += sizeof(unsigned long);

		vertexList.reserve(vertexCount);
		PMD_Vertex* pVertex = (PMD_Vertex*)dataPtr;
		for (int n=0;n<vertexCount;n++,pVertex++) {
			PmdVertex vx;
			vx.pos << pVertex->vec3Pos.x , pVertex->vec3Pos.y , pVertex->vec3Pos.z;
			vx.normal << pVertex->vec3Normal.x , pVertex->vec3Normal.y , pVertex->vec3Normal.z;
			vx.tex << pVertex->uvTex.u , pVertex->uvTex.v;
			vx.weight = pVertex->cbWeight / 100.0f;
			vx.boneNo[0] = pVertex->unBoneNo[0];
			vx.boneNo[1] = pVertex->unBoneNo[1];
			vertexList.append(vx);
		}
		dataPtr += sizeof(PMD_Vertex)*vertexCount;		

		// Load Indices

		unsigned long indexCount = *((unsigned long*)dataPtr);
		dataPtr += sizeof(unsigned long);
		qDebug() << "skipping" << indexCount << "indices";

		dataPtr += sizeof(unsigned short) * indexCount;

		// Load Materials

		int materialCount = *((unsigned long*)dataPtr);
		dataPtr += sizeof(unsigned long);
		qDebug() << "skipping" << materialCount << "materials";

		dataPtr += sizeof(PMD_Material) * materialCount;

		// Load Bones

		int boneCount = *((unsigned short*)dataPtr);
		dataPtr += sizeof(unsigned short);

		boneList.reserve(boneCount);
		for (int n=0;n<boneCount;n++) {
			PMD_Bone* pBone = (PMD_Bone*)dataPtr;
				//QByteArray encodedString(pBone->szName);
				//encodedString.append('\0');
				//QTextCodec* codec = QTextCodec::codecForName("Shift-JIS");
				//qDebug() << "Bone" << codec->toUnicode(encodedString);
			PmdBone bone;
			strncpy(bone.szName, pBone->szName, 20);
			bone.szName[20] = '\0';
			bone.pos << pBone->vec3Position.x , pBone->vec3Position.y , pBone->vec3Position.z;
			bone.parentNo = pBone->nParentNo;
			bone.childNo = pBone->nChildNo;
			bone.kind = pBone->cbKind;

			boneList.append(bone);
			dataPtr += sizeof(PMD_Bone);
		}
		
		for (int n=0;n<boneList.count();n++) {
			PmdBone* bone = &(boneList[n]);
			if (bone->parentNo >= 0 && bone->parentNo < boneList.count()) {
				bone->parent = &(boneList[bone->parentNo]);
				bone->localPos = bone->pos - bone->parent->pos;
			} else {
				qDebug() << "Bone Parent" << bone->parentNo << "not in list (" << boneList.count() << ")";
				bone->localPos = bone->pos;
			}
			if (bone->childNo >= 0 && bone->childNo < boneList.count()) bone->child = &(boneList[bone->childNo]);
			else qDebug() << "Bone Child" << bone->childNo << "not in list (" << boneList.count() << ")";
		}

		free (m_data); m_data = 0;

	}
}

